/* ######################################################################### */
/** \file CollisionMatrix.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class CollisionMatrix
 *
 *  \brief  This is the class of CollisionMatrix function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_CollisionMatrix
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void CollisionMatrix()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static pcl::PointCloud<pcl::PointXYZRGB> CollisionMatrix(DistanceImage str_DistImage,tf::StampedTransform transform)
	{
		std::vector<CollisionPoint> CollisionMatrix;
		pcl::PointCloud<pcl::PointXYZRGB> CollisionPoints;
		pcl::PointCloud<pcl::PointXYZRGB> CollisionPoints_out;
		float fx;
		float fy;
		float fz;
		float phi_min,theta_x_max, phi,theta;

//
//
//		tf::TransformListener listener;
//		tf::StampedTransform transform;
//		tf::StampedTransform transform2;
//		tf::StampedTransform transform3;
//		try{
//			//ROS_INFO("1 transformacion");
//			listener.waitForTransform("/kinect", "/world",	ros::Time(0), ros::Duration(1.0));
//			listener.lookupTransform("/kinect", "/world", ros::Time(0), transform3);
//
//			//ROS_INFO("2 transformacion");
//		}
//		catch (tf::TransformException ex){
//			ROS_ERROR("%s",ex.what());
//		}
		CollisionPoints.height = str_DistImage.s_height;
		CollisionPoints.width = str_DistImage.s_width;
		//CollisionPoints.sensor_origin_ = transform3.getOrigin();
		//CollisionPoints.sensor_orientation_ = transform3.getRotation();


		CollisionPoints.points.resize(str_DistImage.s_height*str_DistImage.s_width);

		phi_min = -str_DistImage.hfoV/2;
		theta_x_max = str_DistImage.vfoV/2; //medido desde x en el plano xz
		for(int i=1;i<=str_DistImage.s_height;i++)
		{
			for(int j =1; j<=str_DistImage.s_width;j++)
			{
				if (str_DistImage.tf_Distance(i,j)>0)
				{
					phi = phi_min + (i-1)*(str_DistImage.hfoV/str_DistImage.s_width); //medido desde x en el plano xy
					theta = ((M_PI/2)-theta_x_max)+(j-1)*(str_DistImage.vfoV/str_DistImage.s_height);//medido desde z en plano xz

					fx = str_DistImage.tf_Distance(i,j)* sin(theta) * cos(phi);
					fy = str_DistImage.tf_Distance(i,j)* sin(theta) * sin(phi);
					fz = str_DistImage.tf_Distance(i,j)* cos(theta);

					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].x = fx;
					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].y = fy;
					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].z = fz;
				}
				else
				{
					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].x = NAN;
					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].y = NAN;
					CollisionPoints.points[(i-1)*str_DistImage.s_width+j-1].z = NAN;
				}
			}
		}

		pcl_ros::transformPointCloud(CollisionPoints,CollisionPoints_out,transform);
		CollisionPoints_out.header = str_DistImage.CollisionPoints.header;
		return CollisionPoints_out;

	}


};
}
